#include "drv_motor.h"
#include "rtthread.h"
#include "rtdevice.h"

/**
 * @brief 装配: 左PB4, 右PE4
 */
#define MOTOR_LEFT_PWM "pwm2"
#define MOTOR_RIGHT_PWM "pwm1"
// freq 500Hz
#define MOTOR_PWM_PERIOD 2000000
static struct rt_device_pwm *motor_left_pwm = RT_NULL;
static struct rt_device_pwm *motor_right_pwm = RT_NULL;

void ist_motor_init(void)
{
    // 初始化电机 PWM
    motor_left_pwm = (struct rt_device_pwm *)rt_device_find(MOTOR_LEFT_PWM);
    motor_right_pwm = (struct rt_device_pwm *)rt_device_find(MOTOR_RIGHT_PWM);

    if (motor_left_pwm == RT_NULL || motor_right_pwm == RT_NULL)
    {
        rt_kprintf("motor_left_pwm or motor_right_pwm not found\n");
        return;
    }

    // Enable pwm
    rt_pwm_enable(motor_left_pwm, 1);
    rt_pwm_enable(motor_right_pwm, 1);
}

void ist_motor_set(int motor_id, double speed)
{
    if (speed > 100)
    {
        speed = 98;
    }
    else if (speed < 0)
    {
        speed = 0;
    }

    if (motor_id == MOTOR_LEFT)
    {
        rt_pwm_set(motor_left_pwm, 1, MOTOR_PWM_PERIOD, MOTOR_PWM_PERIOD * speed / 100);
    }
    else if (motor_id == MOTOR_RIGHT)
    {
        rt_pwm_set(motor_right_pwm, 1, MOTOR_PWM_PERIOD, MOTOR_PWM_PERIOD * speed / 100);
    }
}
